import json
import numpy as np

with open("camera.json", "r") as f:
    data_list = json.load(f)

c2w_list = []

vertices = []
obj_lines = []

for idx, data in enumerate(data_list):
    rot_matrix = data["rot_matrix"]
    trans_matrix = data["trans_matrix"]
    # 构建w2c（world to camera）矩阵，4x4齐次变换矩阵
    rot = np.array(rot_matrix)
    trans = np.array(trans_matrix).reshape(3, 1)
    w2c = np.eye(4)
    w2c[:3, :3] = rot
    w2c[:3, 3] = trans.flatten()
    c2w = np.linalg.inv(w2c)
    c2w_list.append(c2w)

    # 计算相机原点和方向
    origin = np.array([0, 0, 0])
    direction = np.array([0, 0, 1])
    origin_world = np.dot(c2w[:3, :3], origin) + c2w[:3, 3]
    direction_world = np.dot(c2w[:3, :3], direction)
    direction_world = direction_world / np.linalg.norm(direction_world)
    end_point = origin_world + direction_world * 0.1

    # 保存全部相机位姿的顶点，线段索引累加
    vertices.append(origin_world)
    vertices.append(end_point)
    obj_lines.append(f"l {2*idx+1} {2*idx+2}")

# 写入一个总的obj文件
with open("all_cameras_ray.obj", "w") as f:
    for v in vertices:
        f.write("v {} {} {}\n".format(v[0], v[1], v[2]))
    for line in obj_lines:
        f.write(line + "\n")
